package sensor;

import lejos.nxt.Button;
import lejos.nxt.ColorSensor;
import lejos.nxt.ColorSensor.Color;
import lejos.nxt.LCD;
import lejos.nxt.SensorConstants;
import lejos.nxt.SensorPort;
import lejos.nxt.comm.RConsole;
import lejos.util.Delay;

public class SensorColor {
	private int cantColores = 2;
	private int cantValCalib = 8;//4
	private int error = 5;
	private int iterac = 4;//3
	private String[] nombres = new String[2];
	private int[][] colores = new int[cantColores][2];
	private ColorSensor cs = null;//new ColorSensor(SensorPort.S1);
	
	
	public static void main(String[] args) {
		
		//ColorSensor cs = new ColorSensor(SensorPort.S1);
		RConsole.openBluetooth(0);
		SensorColor color = new SensorColor();
		color.calibrar();
		while (true) {
			Button.waitForAnyPress();
			int h = color.obtenerColor();
			RConsole.println(Integer.toString(h));
			LCD.drawInt(h, 0, 0);
		}
	}

	public SensorColor()
	{
		cs = new ColorSensor(SensorPort.S1);	
		cs.setFloodlight(3);
		nombres[0] = "Azul";
		nombres[1] = "Naranja";
		for(int i = 0; i < cantColores; i++)
		{
			for(int j = 0; j < 2; j++)
			{
				colores[i][j] = -1;
			}
		}
		
	}
	
	public void calibrar()
	{
		cs.setFloodlight(Color.RED);
		LCD.drawString ("Search darkness", 0, 0);
		LCD.drawString ("and press Enter", 0, 1);
		Button.waitForAnyPress();
		cs.calibrateLow();
		Delay.msDelay (500);
		LCD.drawString ("Search Ligth", 0, 0);
		LCD.drawString ("and press Enter", 0, 1);
		Button.waitForAnyPress();
		cs.calibrateHigh();
		Delay.msDelay (500);
		cs.setFloodlight(Color.WHITE);
		Delay.msDelay (500);
		LCD.clear();
		for (int i = 0; i < cantColores; i++ )
		{
			for (int j = 0; j < cantValCalib; j ++)
			{
				LCD.drawString("Pelota" + nombres[i] + ":", 0, 0);
				ColorSensor.Color vals = null;
				
				Button.waitForAnyPress();
				
				vals = cs.getColor();
				int r = vals.getRed();
				int g = vals.getGreen();
				int b = vals.getBlue();
				double[] res = rgb2hue(r, g, b);
				int h = (int) res[0];
				if( colores[i][0] == -1 || colores[i][0] > h)
				{
					colores[i][0] = h;
				}
				if( (colores[i][1] == -1) || (colores[i][1] < h))
				{
					colores[i][1] = h;
				}
			}
		}	
		colores[0][0] -= error;
		colores[0][1] += error;
		colores[1][0] -= error;
		colores[1][1] += error;
	}
	
	public int obtenerColor()
	{	
		Color color = null;
		int[] hs = new int[iterac]; 
		for (int i = 0; i < iterac; i++){
			color = cs.getColor();
			int r = color.getRed();
			int g = color.getGreen();
			int b = color.getBlue();
			double[] res = rgb2hue(r, g, b);
			int h = (int) res[0];
			hs[i] = h;
		}	
		RConsole.println("ANTES");
		RConsole.println(Integer.toString(hs[0])+":"+Integer.toString(hs[1]));
		RConsole.println(Integer.toString(colores[0][0])+":"+Integer.toString(colores[0][1]));
		RConsole.println(Integer.toString(colores[1][0])+":"+Integer.toString(colores[1][1]));
		
		
		
		
		int res = -1;
		boolean encontre = false;
		for (int i = 0; i < cantColores && !encontre; i++){
			res = i;
			for(int j = 0; j < iterac; j++){
				if ((colores[i][0] > hs[j]) || (colores[i][1] < hs[j])){
					res = -1;
				}
			}
			if(res != -1)
				encontre = true;
		}
		return res;
	}

	private static double[] rgb2hue(int r, int g, int b) {
		// http://en.wikipedia.org/wiki/HSL_and_HSV
		double[] resultado = new double[3];
		double alpha = (2 * r - g - b) / 2;
		double beta = (Math.sqrt(3) / 2) * (g - b);
		double h = Math.atan2(beta, alpha);
		if (h < 0)
			h = h + 2 * Math.PI;
		h = Math.round((h * 360) / (Math.PI * 2));
		double s = Math
				.round(Math.sqrt(Math.pow(alpha, 2) + Math.pow(beta, 2)));
		double i = (double) (r + g + b) / 3;
		resultado[0] = h;
		resultado[1] = s;
		resultado[2] = i;
		return resultado;
	}


}
